﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
//using System.IO.Ports;
using OpenNETCF.IO.Ports;
using System.Threading;
using System.Runtime.InteropServices;
using Microsoft.DirectX.DirectInput;
using Microsoft.DirectX;
using System.Windows.Forms.DataVisualization.Charting;
using System.IO;
using System.Text.RegularExpressions;

namespace robotGUI
{
    public partial class frmMain : Form
    {
        //Global variables
        public robotSettings Settings;
        Ports availablePorts;

        //First delegate gets messages from port and marshalls back to the UI thread
        delegate void UpdateChartDelegate(robotMsg msg);
        UpdateChartDelegate updateChart;

        robotSettings.ParameterArrivedEventHandler gotBattRaw;

        SerialPort port { get { return serialControl.Port; } set { serialControl.Port = value; } }

        //Status string
        char[] trimChars = new char[] { '\r', '\n', ':' };
        public string Status
        {
            set
            {
                string line = value;
                if (line.Contains('\n')) line = line.Substring(0,line.IndexOf('\n'));
                lblStatus.Text = line.Trim(trimChars);
                termControl1.Append(value);
            }
        }

        //------------------------------------------------------------------------
        //Constructor and initialization
        frmHeartbeat frmHeart;
        CheckBox[] abilityCheckBoxes;
        public frmMain()
        {
            InitializeComponent();
            dxAngle.Owner = this;           //Give dxAngle a route back to it's container
            availablePorts = new Ports();   //Create RS232 port interface

        }

        private void frmMain_Load(object sender, EventArgs e)
        {
            //Settings dialog
            Settings = new robotSettings(serialControl.serialControl);
            Settings.PropertyChanged += new PropertyChangedEventHandler(Settings_PropertyChanged);
            gotBattRaw = new robotSettings.ParameterArrivedEventHandler(Settings_gotBattRaw);
            robotSettingsBindingSource.DataSource = Settings;

            //Binding battVoltage = new Binding("Text", this.robotSettingsBindingSource, "BATT_LOW_VOLTAGE", true, System.Windows.Forms.DataSourceUpdateMode.OnPropertyChanged, null, "#0.##v");
            //battVoltage.Parse += new ConvertEventHandler(battVoltage_Parse);
            //textBox2.DataBindings.Add(battVoltage);

            
            //Register the Settings messagehandler
            NewMessage += new NewMessageEventHandler(Settings.newMessage);
            //serial events
            serialControl.DataReceived += new serialControl.DataReceivedEventHandler(serialControl_DataReceived);
            serialControl.PortError += new serialControl.PortErrorEventHandler(serialControl_PortError);
            //serial delegates
            updateChart = new UpdateChartDelegate(UpdateChart);
            //Form events
            //Retrieve some properties
 //           numOutput.Value = Properties.Settings.Default.graph3width;
 //           numMotorPID.Value = Properties.Settings.Default.graph4width;
            //dxAngle.WorldLocation = Matrix.RotationY((float)(Math.PI / 2F));
            //cbxPid.SelectedIndex = Properties.Settings.Default.pidIndex;
            abilityCheckBoxes = new CheckBox[] {
                chkRC,
                chkIMU,
                chkEncoders,
                chkMotorPID,
                chkDrift,
                chkRoll,
                chkBalance,
                chkPoshold
            };
            setAbilities(Settings.ABILITY_ENABLE);

            tabControl1.SelectedIndex = -1;
            tabControl1.SelectedIndex = Properties.Settings.Default.selectedTab;
            //dataRequest = robotMsg.DataType.REQUEST_PID_NONE;
            //Settings.DATA_REQUEST.Set((byte)dataRequest);


        }

        //-----------------------------------------------------------------------
        //Serial port

        private void serialControl_PortOpening(object sender, EventArgs e)
        {
            Status = "COM Port: Opening COM Port..";
            this.Cursor = Cursors.WaitCursor;
        }

        int heartBeatTimeout = 30;
        int heartBeatTimer;
        bool gotHeartbeat;
        private void serialControl_PortOpened(object sender, EventArgs e)
        {
            Status = "COM Port: Opened.";
            cbxMode.Enabled = true;
            this.Cursor = Cursors.Default;
            heartTimer.Start();
            heartBeatTimer = heartBeatTimeout;
            gotHeartbeat = false;
            //this.Refresh();
            //Settings.ParameterArrived += Settings_ParameterArrived;
            Settings.getValue(Command.PULSES_PER_METER);
        }

        //Serial Port error 
        void serialControl_PortError(object sender, PortErrorEventArgs e)
        {
            string message = "COM Error: " + e.Message;
            if (!this.Disposing && !this.IsDisposed && this.IsHandleCreated)
                this.Invoke((MethodInvoker)delegate { Status = message; });
        }

        private void serialControl_PortClosed(object sender, EventArgs e)
        {
            this.Cursor = Cursors.Default;
            Status = "COM Port: Closed.";
            currentDrivingMode = -1;
            cbxMode.SelectedIndex = -1;
            cbxMode.Text = "Not Connected";
            cbxMode.Enabled = false;
            frmHeart_Close();
            Settings.Invalidate();
            joyControl1.EventsEnabled = false;
        }

        //Force frmHeart to close
        public void frmHeart_Close()
        {
            heartTimer.Stop(); //also stop timer
            if (frmHeart != null && !frmHeart.IsDisposed) frmHeart.Close();
        }

        private void heartTimer_Tick(object sender, EventArgs e)
        {
            //Serial port closed, timer ticking? - close frmHeart
            if (!serialControl.IsOpen)                
                frmHeart_Close();
            //count down - popup form after 5 seconds
            else if (--heartBeatTimer < heartBeatTimeout - 4)
            {
                if (frmHeart == null || frmHeart.IsDisposed)
                {
                    Status = "COM Port: Heartbeat Timeout..";
                    frmHeart = new frmHeartbeat();
                    frmHeart.FormClosed += new FormClosedEventHandler(frmHeart_FormClosed);
                    frmHeart.Show(this);
                }
                frmHeart.Timeout = heartBeatTimer.ToString();
            }
            //Timeout - close serial port
            if (heartBeatTimer == 0)
            {
                frmHeart_Close();
                serialControl.Close();
            }
        }

        //User may have cancelled form if !gotHeartbeat
        void frmHeart_FormClosed(object sender, FormClosedEventArgs e)
        {
            heartTimer.Stop(); //ensure timer stopped
            if (!gotHeartbeat) serialControl.Close(); //no heartbeat..
        }

        //Called on receiving message to reset heartbeat timer
        void heartTimerReset()
        {
            if (frmHeart != null && !frmHeart.IsDisposed)
            {
                Status = "COM Port: Got Heartbeat.";
                gotHeartbeat = true;
                frmHeart.Close();
            }
            heartTimer.Stop();
            heartTimer.Start();
            heartBeatTimer = heartBeatTimeout;
        }
        
        //Recieve data from port thread and send lines to UI thread
        robotMsg msg = new robotMsg();
        StringBuilder line = new StringBuilder();
        bool cr = false;
        void serialControl_DataReceived(object sender, SerialReceivedEventArgs e)
        {
            while (port != null && port.IsOpen && port.BytesToRead > 0)
            {
                string data = port.ReadExisting();
                for (int index = 0; index < data.Length; index++)
                {
                    if (data[index] != '\r' && data[index] != '\n')
                    {
                        if (data[index] != '\0') line.Append(data[index]);
                        cr = false;
                    }
                    else if (!cr)
                    {
                        line.Append(data[index]);
                        string gotLine = line.ToString();
                        if (!this.Disposing && !this.IsDisposed)
                            this.Invoke((MethodInvoker)delegate { lineArrived(gotLine); });
                        line.Length = 0;
                        cr = true;
                    }
                }
            }
        }

        //Incoming line - allow robotMsg to parse it, and dispatch
        void lineArrived(string message)
        {
            msg.Update(message.ToString());
            UpdateChart(msg);
            heartTimerReset();
            OnNewMessage(msg);
        }

        public delegate void NewMessageEventHandler(object sender, NewMessageEventArgs e);
        public event NewMessageEventHandler NewMessage;
        public void OnNewMessage(robotMsg msg)
        {
            if (NewMessage != null)
            {
                NewMessage(this, new NewMessageEventArgs(msg));
            }
        }

        //---------------------------------------------------------------------------------------
        //message -> GUI
        int currentDrivingMode = -1;
        robotMsg.DataType dataRequest;
        //decimal pVal, iVal, dVal, driftRate;
        int batteryValue = 0;
        int oldPercent = 0;
        int oldSpeed = 0;
        void UpdateChart(robotMsg msg)
        {
            //try
            //{
                //Error message
                if (msg.msgType == robotMsg.Type.ERR)
                {
                    Status = "ERROR: message not recognised!\n" + msg.status;
                }
                //Status message
                if (msg.msgType == robotMsg.Type.STAT)
                {
                    if (Settings.USER_NAME == "") Status = "(Remote): " + msg.status; //Don't know name yet..
                    else Status = Settings.USER_NAME + ": " + msg.status;
                    //Scoop up user name early during initial connection
                    if (msg.status.StartsWith("Name: ")) Settings.USER_NAME = msg.status.Substring(6);
                }
                else if (msg.msgType == robotMsg.Type.SEN)
                {
                    //Raw data
                    graphRaw.Add("ACC_X", msg.ACC_X);
                    graphRaw.Add("ACC_Y", msg.ACC_Y);
                    graphRaw.Add("ACC_Z", msg.ACC_Z);
                    graphRaw.Add("GYR_X", msg.GYR_X);
                    graphRaw.Add("GYR_Y", msg.GYR_Y);
                    graphRaw.Add("GYR_Z", msg.GYR_Z);
                    //Raw angles
                    graphKalman.Add("ACC_pitch", msg.ACC_pitch);
                    graphKalman.Add("ACC_roll", msg.ACC_roll);
                }

                else if (msg.msgType == robotMsg.Type.MOV)
                {
                    if (tabControl1.SelectedTab == tabPageSensors)
                    {
                        graphKalman.Add("pitchAngle", msg.pitchAngle);
                        graphKalman.Add("rollAngle", msg.rollAngle);
                    }
                    if (tabControl1.SelectedTab == tabPageBalance)
                    {
                        graphAngle.Add("Actual", msg.pitchAngle);
                    }
                    int pitch = (int)(msg.pitchAngle * (90F / 256F));
                    if (pitch > 180) pitch -= 360;
                    if (pitch < -180) pitch += 360;
                    int roll = (int)(msg.rollAngle * (90F / 256F));
                    if (roll > 180) roll -= 360;
                    if (roll < -180) roll += 360;
                    //3D car
                    dxAngle.CarPitch = pitch + 90;
                    dxAngle.CarRoll = -roll;
                    //dashboard
                    dashControl1.dxControl1.CarPitch = pitch + 90;
                    dashControl1.dxControl1.CarRoll = -roll;
                    if (Settings.PULSES_PER_METER > 0)
                    {
                        dashControl1.dxControl1.LeftWheelDistance += (msg.current_speed_L_motor / Settings.PULSES_PER_METER) * 80;
                        dashControl1.dxControl1.RightWheelDistance += (msg.current_speed_R_motor / Settings.PULSES_PER_METER) * 80;
                        dxAngle.LeftWheelDistance += (msg.current_speed_L_motor / Settings.PULSES_PER_METER) * 80;
                        dxAngle.RightWheelDistance += (msg.current_speed_R_motor / Settings.PULSES_PER_METER) * 80;
                    }
                    dxAngle.Invalidate();
                    dashControl1.dxControl1.Invalidate();
                    //attitude indicator
                    dashControl1.attitudeIndicator1.SetAttitudeIndicatorParameters(pitch, roll);
                    //speed dial
                    float speed = (msg.current_speed_L_motor + msg.current_speed_R_motor) / 2;
                    speed = (speed / Settings.PULSES_PER_METER) * 80;
                    if ((int)speed != oldSpeed)
                    {
                        dashControl1.speedIndicator1.SetAirSpeedIndicatorParameters((int)Math.Abs(speed));
                        oldSpeed = (int)speed;
                    }
                }

                //Standard message - soon to be removed
                else if (msg.msgType == robotMsg.Type.STD)
                {
                    //Angles
                    //if (chartAngle.Series["Series1"].Points.Count >= numAngle.Value) chartAngle.Series["Series1"].Points.RemoveAt(0);
                    //if (chartAngle.Series["Series2"].Points.Count >= numAngle.Value) chartAngle.Series["Series2"].Points.RemoveAt(0);
                    //chartAngle.Series["Series1"].Points.Add(msg.ACC_pitch);
                    //chartAngle.Series["Series2"].Points.Add(msg.pitchAngle);
                    //chartAngle.ResetAutoValues();
                    //Horizon
                    int angle = (int)(msg.pitchAngle * (90F / 256F));
                    if (angle > 180) angle -= 360;
                    if (angle < -180) angle += 360;
                    int roll = (int)(msg.rollAngle * (90F / 256F));
                    if (roll > 180) roll -= 360;
                    if (roll < -180) roll += 360;
                    dxAngle.CarPitch = angle + 90;
                    dxAngle.CarRoll = -roll;
                    dxAngle.Invalidate();
                    dashControl1.attitudeIndicator1.SetAttitudeIndicatorParameters(angle, roll);
                    //Raw data
                    graphRaw.Add("ACC_X", msg.ACC_X);
                    graphRaw.Add("ACC_Z", msg.ACC_Z);
                    graphRaw.Add("GYR_Y", msg.GYR_Y);
                    //Loop times
                    labelValue1.Value = msg.STD_LOOP_TIME.ToString() + " ms";
                    labelValue2.Value = msg.lastLoopTime.ToString("F1");
                    labelValue3.Value = msg.lastLoopUsefulTime.ToString("F1");
                    labelValue4.Value = msg.updateRate.ToString();
                    //Balance PID

                    //if (Properties.Settings.Default.pidIndex == 1)
                    //{
                    //numBalanceP.Value = pVal = (decimal)msg.bal_Kp;
                    //numBalanceI.Value = iVal = (decimal)msg.bal_Ki;
                    //numBalanceD.Value = dVal = (decimal)msg.bal_Kd;
                    //if (chartBalancePID.Series["P"].Points.Count >= numBalancePID.Value) chartBalancePID.Series["P"].Points.RemoveAt(0);
                    //if (chartBalancePID.Series["I"].Points.Count >= numBalancePID.Value) chartBalancePID.Series["I"].Points.RemoveAt(0);
                    //if (chartBalancePID.Series["D"].Points.Count >= numBalancePID.Value) chartBalancePID.Series["D"].Points.RemoveAt(0);
                    //if (chartBalancePID.Series["setPoint"].Points.Count >= numBalancePID.Value) chartBalancePID.Series["setPoint"].Points.RemoveAt(0);
                    //chartBalancePID.Add("P", msg.pTerm);
                    //chartBalancePID.Add("I", msg.iTerm);
                    //chartBalancePID.Add("D", msg.dTerm);
                    //chartBalancePID.Add("setPoint", msg.setPoint);
                    //}

                }

                //Motor drive
                else if (msg.msgType == robotMsg.Type.MOT)
                {
                    graphOutput.Add("Left", msg.leftPWM);
                    graphOutput.Add("Right", msg.rightPWM);
                }

                //PID message
                else if (msg.msgType == robotMsg.Type.PID)
                {
                    if (((int)dataRequest & 7) == (int)robotMsg.DataType.REQUEST_PID_LMOTOR)
                    {
                        graphMotor.Add("Input", msg.lm_input);
                        graphMotor.Add("SetPoint", msg.lm_setPoint);
                        graphMotor.Add("P", msg.lm_pTerm);
                        graphMotor.Add("I", msg.lm_iTerm);
                        graphMotor.Add("D", msg.lm_dTerm);
                        graphMotor.Add("Output", msg.lm_output);
                    }
                    if (((int)dataRequest & 7) == (int)robotMsg.DataType.REQUEST_PID_RMOTOR)
                    {
                        graphMotor.Add("Input", msg.rm_input);
                        graphMotor.Add("SetPoint", msg.rm_setPoint);
                        graphMotor.Add("P", msg.rm_pTerm);
                        graphMotor.Add("I", msg.rm_iTerm);
                        graphMotor.Add("D", msg.rm_dTerm);
                        graphMotor.Add("Output", msg.rm_output);
                    }
                    if (((int)dataRequest & 7) == (int)robotMsg.DataType.REQUEST_PID_BALANCE)
                    {
                        graphBalance.Add("Input", msg.bal_input);
                        graphBalance.Add("SetPoint", msg.bal_setPoint);
                        graphBalance.Add("P", msg.bal_pTerm);
                        graphBalance.Add("I", msg.bal_iTerm);
                        graphBalance.Add("D", msg.bal_dTerm);
                        graphBalance.Add("Output", msg.bal_output);
                        graphAngle.Add("Desired", msg.bal_setPoint);
                        graphAngle.Add("Correction", msg.bal_error);
                    }
                }
                //Heartbeat message
                else if (msg.msgType == robotMsg.Type.HB)
                {
                    joyControl1.EventsEnabled = true;
                    serialControl.HeartBeat();
                    //Failsafe
                    if (msg.failsafeFlags == 0)
                    {
                        rcDisplay1.LeftColor = Color.Tomato;
                        rcDisplay1.RightColor = Color.Red;
                        dashControl1.rcDisplay2.LeftColor = Color.Tomato;
                        dashControl1.rcDisplay2.RightColor = Color.Red;
                        picSignal.Image = Properties.Resources.antenna1;
                    }
                    else
                    {
                        rcDisplay1.LeftColor = Color.DarkTurquoise;
                        rcDisplay1.RightColor = Color.Blue;
                        dashControl1.rcDisplay2.LeftColor = Color.DarkTurquoise;
                        dashControl1.rcDisplay2.RightColor = Color.Blue;
                        picSignal.Image = Properties.Resources.antenna2;
                    }
                    dashControl1.indicatorRC.State = ((msg.failsafeFlags & (int)failsafe.RC) > 0);
                    dashControl1.indicatorJOY.State = ((msg.failsafeFlags & (int)failsafe.SERIAL) > 0);
                    //battery
                    string battVoltage = msg.batteryVoltage.ToString("F1") + "v";
                    labelValue5.Value = battVoltage;
                    battLevel1.Extra = battVoltage;
                    if (msg.batteryVoltage <= 6.8 && batteryValue != 0)
                    {
                        labelValue5.ValueColor = Color.Red;
                        lblBattery.ForeColor = Color.Red;
                        picBattery.Image = Properties.Resources.battery1;
                        batteryValue = 0;
                    }
                    else if (msg.batteryVoltage > 6.8)
                    {
                        if (batteryValue == 0)
                        {
                            labelValue5.ValueColor = Color.Black;
                            lblBattery.ForeColor = Color.Black;
                        }
                        if (msg.batteryVoltage < 7.2)
                        {
                            if (batteryValue != 1)
                            {
                                picBattery.Image = Properties.Resources.battery2;
                                batteryValue = 1;
                            }
                        }
                        else if (msg.batteryVoltage < 7.6)
                        {
                            if (batteryValue != 2)
                            {
                                picBattery.Image = Properties.Resources.battery3;
                                batteryValue = 2;
                            }
                        }
                        else if (msg.batteryVoltage < 8.0)
                        {
                            if (batteryValue != 3)
                            {
                                picBattery.Image = Properties.Resources.battery4;
                                batteryValue = 3;
                            }
                        }
                        else if (msg.batteryVoltage < 9.0)
                        {
                            if (batteryValue != 4)
                            {
                                picBattery.Image = Properties.Resources.battery5;
                                batteryValue = 4;
                            }
                        }
                        else if (batteryValue != 5)
                        {
                            picBattery.Image = Properties.Resources.ACpower;
                            batteryValue = 5;
                        }
                    }
                    if (msg.batteryVoltage > 9.0F) lblBatHeader.Text = "DC Supply";
                    else lblBatHeader.Text = "Battery";
                    int percent = (int)(((msg.batteryVoltage - 6.0F) / 2.4F) * 100F);
                    if (percent < 0) percent = 0;
                    if (percent > 100) percent = 100;
                    if (percent != oldPercent)
                    {
                        dashControl1.battLevel1.Percent = percent;
                        battLevel1.Percent = percent;
                        lblBattery.Text = percent.ToString("F0") + "%";
                        prgBattery.Value = (int)percent;
                        oldPercent = percent;
                    }
                    if (currentDrivingMode != (int)msg.drivingMode)
                    {
                        currentDrivingMode = (int)msg.drivingMode;
                        cbxMode.SelectedIndex = (int)msg.drivingMode;
                    }
                    if (dataRequest != (robotMsg.DataType)msg.dataRequest)
                    {
                        Settings.DATA_REQUEST.Set((byte)dataRequest);
                        Settings.Resend(Command.DATA_REQUEST);
                        Console.WriteLine("Resync DATA_REQUEST");
                    }
                }
                //RC message
                else if (msg.msgType == robotMsg.Type.RC)
                {
                    //Rc data
                    rcDisplay1.Value1 = (int)msg.rcData1;
                    rcDisplay1.Value2 = (int)msg.rcData2;
                    rcDisplay1.Value3 = (int)msg.rcData3;
                    rcDisplay1.Value4 = (int)msg.rcData4;
                    dashControl1.rcDisplay2.Value1 = (int)msg.rcData1;
                    dashControl1.rcDisplay2.Value2 = (int)msg.rcData2;
                    dashControl1.rcDisplay2.Value3 = (int)msg.rcData3;
                    dashControl1.rcDisplay2.Value4 = (int)msg.rcData4;
                    labelValue6.Value = msg.driftPercent.ToString() + "%";
                    numRate.Value = (int)msg.driftRate;
                }
                else if (msg.msgType == robotMsg.Type.TIM)
                {

                    lblTotalLoop.Text = msg.lastLoopTime.ToString("F1") + " ms";
                    lblUsedLoop.Text = msg.lastLoopUsefulTime.ToString("F1") + " ms";
                    int percent = (int)((msg.lastLoopUsefulTime / msg.lastLoopTime) * 100f);
                    dashControl1.cpUgraph1.Add(percent);
                    cpUgraph1.Add(percent);
                }
                if (port.IsOpen && port.BytesToRead > 1024) Status = "Communication lag! " + port.BytesToRead.ToString();
            //}
            //catch (Exception ex)
            //{
                //want "in 'object'" - see if targetsite can name control responsible
            //    Status = "ERROR: Malformed message caused error?:\n" + ex.Message + "\n" + ex.StackTrace;
            //}
        }

        //========================================================================
        //UI controls

        private void tabControl1_SelectedIndexChanged(object sender, EventArgs e)
        {
            if (tabControl1.SelectedTab == tabPageSetup)
            {
                if (pageControl1.WorkingArea.SelectedTab == pageJoystick)
                    dataRequest = robotMsg.DataType.REQUEST_RC;
                else if (pageControl1.WorkingArea.SelectedTab == pageTiming)
                    dataRequest = robotMsg.DataType.REQUEST_TIMING;
                else dataRequest = robotMsg.DataType.REQUEST_PID_NONE;
            }
            else if (tabControl1.SelectedTab == tabPageDashboard)
                dataRequest = robotMsg.DataType.REQUEST_MOVEMENT | robotMsg.DataType.REQUEST_TIMING | robotMsg.DataType.REQUEST_RC;
            else if (tabControl1.SelectedTab == tabPageSensors)
                dataRequest = robotMsg.DataType.REQUEST_MOVEMENT | robotMsg.DataType.REQUEST_SENSORS;
            else if (tabControl1.SelectedTab == tabPageControls)
                dataRequest = robotMsg.DataType.REQUEST_RC;
            else if (tabControl1.SelectedTab == tabPageMotors)
                dataRequest = robotMsg.DataType.REQUEST_MOTORS | robotMsg.DataType.REQUEST_PID_LMOTOR;
            else if (tabControl1.SelectedTab == tabPageBalance)
                dataRequest = robotMsg.DataType.REQUEST_MOVEMENT | robotMsg.DataType.REQUEST_PID_BALANCE;
            Settings.DATA_REQUEST.Set((byte)dataRequest);
        }

        private void frmMain_FormClosing(object sender, FormClosingEventArgs e)
        {
            serialControl.ForceClose();
            joyControl1.EventsEnabled = false;
            Properties.Settings.Default.Save();
        }

        void requestData(robotMsg.DataType data)
        {
            Settings.DATA_REQUEST.Set((byte)(dataRequest = data));
        }

        private void btnCalSpeed_Click(object sender, EventArgs e)
        {
            robotMsg.DataType currentRequest = dataRequest;
            frmSpeedCal speedForm = new frmSpeedCal();
            requestData(robotMsg.DataType.REQUEST_MOTORS | robotMsg.DataType.REQUEST_MOVEMENT);
            speedForm.ShowDialog(this);
            requestData(currentRequest);
        }

        private void btnSettings_Click(object sender, EventArgs e)
        {
            //if (port == null || !port.IsOpen) return;
            frmSettings settings = new frmSettings(Settings);
            settings.Show(this);
        }

        //Used by dxDisplay to provide aspect ratio lock
        public void resizeViewport(int width)
        {
            //tableLayoutPanel10.ColumnStyles[1].Width = width + 2;
            splitContainer1.SplitterDistance = splitContainer1.Width - (width + 5);
            this.Refresh();
        }

        private void joyDisplay1_SetDeadband(object sender, SetDeadbandEventArgs e)
        {
            Properties.Settings.Default.deadBand = e.DeadBand;
        }

        //======================================================================
        
        private void eeEdit_Click(object sender, EventArgs e)
        {
            frmEepromEditor editor = new frmEepromEditor();
            
            editor.Show(this);
        }

        //Switch driving mode
        private void cbxMode_SelectedIndexChanged(object sender, EventArgs e)
        {
            serialControl.sendValue(Command.DRIVING_MODE, (float)cbxMode.SelectedIndex);
            currentDrivingMode = cbxMode.SelectedIndex;
        }

        void Settings_PropertyChanged(object sender, PropertyChangedEventArgs e)
        {
            robotSettings.CommandChangedEventArgs args = (robotSettings.CommandChangedEventArgs)e;
            if (args.command == Command.LEFTMOTORPID_KP) numMotorP.Value = (decimal)Settings.LEFTMOTOR.P;
            if (args.command == Command.LEFTMOTORPID_KI) numMotorI.Value = (decimal)Settings.LEFTMOTOR.I;
            if (args.command == Command.LEFTMOTORPID_KD) numMotorD.Value = (decimal)Settings.LEFTMOTOR.D;
            if (args.command == Command.ABILITY_ENABLE) 
                setAbilities(Settings.ABILITY_ENABLE);
        }

        private void setAbilities(robotSettings.Abilities abilityEnable)
        {
           foreach (CheckBox check in abilityCheckBoxes) 
               check.Checked = ((abilityEnable.Get() & int.Parse((string)check.Tag)) > 0);
        }

        private void abilityCheck_CheckedChanged(object sender, EventArgs e)
        {
            int value = 0;
            foreach (CheckBox check in abilityCheckBoxes)
                if (check.Checked) value += int.Parse((string)check.Tag);
            Settings.ABILITY_ENABLE.Set(value);
        }

        private void numMotorP_ValueChanged(object sender, EventArgs e)
        {
            Settings.RIGHTMOTOR.P = (float)numMotorP.Value;
        }

        private void numMotorI_ValueChanged(object sender, EventArgs e)
        {
            Settings.RIGHTMOTOR.I = (float)numMotorI.Value;
        }

        private void numMotorD_ValueChanged(object sender, EventArgs e)
        {
            Settings.RIGHTMOTOR.D = (float)numMotorD.Value;
        }

        byte[] joyCommand;
        private void joyControl1_JoyChanged(object sender, joyControl.JoyChangedEventArgs e)
        {
            joyCommand = new byte[] { (byte)(e.X / 4), (byte)(-e.Y / 4), 0, 0 };
            serialControl.sendValue(Command.SERIAL_CONTROL, joyCommand);
        }

        private void Reset_Click(object sender, EventArgs e)
        {
            if (serialControl.IsOpen)
            {
                Status = "Sending reset..";
                serialControl.sendValue(Command.SOFT_RESET, robotMsg.DOOMSDAY_MAGIC);
            }
        }

        //Battery Calibration
        float battMeasure;
        private void txtBattMeasure_TextChanged(object sender, EventArgs e)
        {
            btnSetBatt.Enabled = (float.TryParse(txtBattMeasure.Text, out battMeasure) && battMeasure >= 1);
        }

        private void btnSetBatt_Click(object sender, EventArgs e)
        {
            if (float.TryParse(txtBattMeasure.Text, out battMeasure) && battMeasure >= 1) {
              Settings.ParameterArrived += gotBattRaw;
              Settings.getValue(Command.BATT_RAW);
            }
        }

        void Settings_gotBattRaw(object sender, robotSettings.ParameterEventArgs e)
        {
            if (e.command == Command.BATT_RAW)
            {
                Settings.ParameterArrived -= gotBattRaw;
                float factor = battMeasure / Settings.BATT_RAW;
                Settings.BATT_FACTOR = factor;
            }
        }

        private void cbxMode_KeyPress(object sender, KeyPressEventArgs e)
        {
            e.Handled = true;
        }

    }
    //Outgoing new message event
    public class NewMessageEventArgs : EventArgs
    {
        public robotMsg msg;
        public NewMessageEventArgs(robotMsg msg)
        {
            this.msg = msg;
        }
    }
}



